#include "pid.h"
#include "mpu6050_dmp.h"
#include "encoder.h"
#include "motor.h"

int MOTO1,MOTO2;

// 期望速度
int16_t Traget_Speed = 0;
// 期望转动的角度
float Traget_turn = 0.0f;
// 停止标志位
uint8_t Stop_Flag = 0;
// 误差修正角
int Med_Angle = 5.5f;

/* 极性的方向与设定的电机的参考正方向有关 */

// 直立环PD控制器                                            // 调至超调，然后*0.6
float Vertical_Kp = -18.0f,Vertical_Kd = 0.075f;            // 数量级：Kp：0~100，Kd:0~1

// 已近比较接近了，然后有比较大的高频抖动

// 速度环PI控制器
float Velocity_Kp = 1.0f,Velocity_Ki = 0.005f;        // 数量级：Kp:0~1    Ki：Kp / 200

// 转向环PI控制器
float Turn_Kp = 0.0f, Turn_Kd = 0.0f;                // Kd修正角度 0~1

// 直立环PD控制器
// 输入:期望角度，实际角度，角速度
int16_t Car_Upright_PD(float Med,float Angle,int16_t gyre_y)
{
	int16_t temp;
	//陀螺仪角度的偏向相对正方向也会影响极性
	temp = Vertical_Kp * (Angle - Med) + Vertical_Kd * gyre_y;

	return temp;
}

// 速度环PI控制器
// 输入:期望速度，左编码器，右编码器
// 输入的速度是以编码器的值相对来定义的
int16_t Car_Velocity_PI(int16_t Target,int16_t encoder_L,int16_t encoder_R)
{
    // 误差存储
	static int Err_LowOut_last;
    // 积分值存储
    static int Encoder_s;           
    // 低通滤波系数
	static float Factor = 0.7;

	int Err,Err_LowOut;

	//1.计算差值
	Err = (encoder_L + encoder_R) - Target;

	//2.低通滤波
	Err_LowOut = (1 - Factor) * Err  + Factor * Err_LowOut_last;
	Err_LowOut_last = Err_LowOut;

	//3.积分
	Encoder_s += Err_LowOut;

    // 掉时候再重新调整
	//4.积分限幅(-20000~20000)
	Encoder_s = Encoder_s > 20000 ? 20000:(Encoder_s <(-20000)?(-20000):Encoder_s);

    if(Stop_Flag == 1) Encoder_s = 0,Stop_Flag = 0;              // 停车

    // //===速度控制  
    // if(Pitch_dmp < -40||Pitch_dmp > 40)   Encoder_s = 0;   
    // //===电机关闭后清除积分

	//5,速度环计算
	int16_t temp = Velocity_Kp * Err_LowOut + Velocity_Ki * Encoder_s;

	return temp;
}

// 转向环PI控制器
// 输入：角度值,角速度
int16_t Car_Turn_PI(float Target_turn ,float gyro_z)
{
	int16_t temp;
	temp = Turn_Kp * Target_turn + Turn_Kd * gyro_z;
	return temp;
}

//每10ms调用一次
// 平衡车PID控制
void Car_PID(void)
{
	int16_t PWM_out;

	//2.控制器联动
	int16_t Velocity_out = Car_Velocity_PI(Traget_Speed,Encoder_Left,Encoder_Right);          // 速度环计算

	int16_t Vertial_out = Car_Upright_PD(Velocity_out + Med_Angle, Pitch_dmp , gy_dmp);       // 直立环计算

	int Turn_out = Car_Turn_PI(gz_dmp,Traget_turn);                                           // 转向环计算

	PWM_out = Vertial_out;

    MOTO1 = PWM_out - Turn_out;
    MOTO2 = PWM_out + Turn_out;

	//输出限幅
	if(MOTO1 > 1023)
	{
		MOTO1 = 1023;
	}
	if(MOTO1 < -1023)
	{
		MOTO1 = -1023;
	}

	if(MOTO2 > 1023)
	{
		MOTO2 = 1023;
	}
	if(MOTO2 < -1023)
	{
		MOTO2 = -1023;
	}

	//中间再加一个限幅
	Motor_Set(Motor_Left,  MOTO1);
	Motor_Set(Motor_Right, MOTO2);
}
